#include <cstdio>
#include <vector>
#include "sift/kdtree.h"
#include "sift/imgfeatures.h"
#include "featurematcher.h"
#include "imagefeature.h"
#include "stitcher.h"
#include "imageset.h"
#include <boost/foreach.hpp>
#include "inputimage.h"

using namespace boost;
using namespace cshotel;
using namespace std;
using namespace sift;

FeatureMatcher::FeatureMatcher(shared_ptr<ImageSet> imgs) : imageSet(imgs) { }

void FeatureMatcher::match(int numNeighbours) {
    dout("Matching features ..." << endl);
    
    // copy features to one vector
    vector<shared_ptr<ImageFeature> > features;
    foreach(shared_ptr<InputImage> img, imageSet->getImages()) {
        features.insert(features.end(), img->getImageFeatures().begin(), img->getImageFeatures().end());
    }
    
    dout("    building k-d-tree ..." << endl);
    
    // build k-d-tree from the extracted features
    feature* siftFeatures = new feature[features.size()];
    for(int ii = 0; ii < features.size(); ++ii) {
        siftFeatures[ii] = features[ii]->getSIFTFeature();
        siftFeatures[ii].feature_data = new int(ii);
    }
    kd_node* root = kdtree_build(siftFeatures, features.size());
    
    dout("    finding closest features ... ");
    feature **neighbours;
    int percentStep = features.size() / 100;
    int nextStep = 1;
    
    // iterate over all features
    for(int ii = 0; ii < features.size(); ++ii) {
        // for each feature, find the closest matches
        int numFound = kdtree_bbf_knn(root, &(features[ii]->getSIFTFeature()), numNeighbours, &neighbours, max_nn_chks);
        for(int jj = 0; jj < numFound; ++jj) {
            features[ii]->addSIFTFeatureMatch(features[*static_cast<int*>(neighbours[jj]->feature_data)]);
        }
        
        if(ii > nextStep * percentStep) {
            dout(nextStep << "%  ");
            nextStep += 1;
        }
    }
    dout(" done" << endl);
    
    // clean up
    kdtree_release(root);
    for(int ii = 0; ii < features.size(); ++ii) {
        delete static_cast<int*>(siftFeatures[ii].feature_data);
    }
    delete[] siftFeatures;
}
